#include <nuttx/config.h>
#include <errno.h>
#include <string.h>
#include <fcntl.h>
#include <stdio.h>
#include <stdlib.h>
#include <visibility.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <uORB/uORB.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/rc_channels.h>
#include <mavlink/mavlink_log.h>
#include <common/Thread.h>
#include "gyro_calibration.h"
#include "accelerometer_calibration.h"
#include "mag_calibration.h"
#include "zorfcsadapter.h"
#include "zorfcs/slowloop.h"
#include "zorfcs/fastloop.h"
#include "zorfcs/eventmanager.h"
#include "zorfcs/filestorage.h"
#include "zorfcs/parammanager.h"
#include "zorfcs/waypointmanager.h"
#include "zorfcs/route.h"
#include "zorfcs/manager.h"
#include "zorfcs/wow.h"
#include "test.h"


extern "C" __EXPORT int fcs_main(int argc, char *argv[]);
extern "C" __EXPORT int fcs_thread_main(int argc, char *argv[]);

namespace {

class App {
public:
	static void create() {
		destroy();
		instance_ = new App();
		while (instance_ != NULL)
			Thread::sleep(100);
	}

	static void destroy() {
		delete instance_;
		instance_ = NULL;
	}

	static bool isRunning() {
		return instance_ != NULL;
	}

private:
	static App *instance_;

	App()
		: storage_(new FileStorage("/fs/microsd/fram.bin", 128 * 1024))
		, paramManager_(new ParamManager(storage_))
		, waypointManager_(new WaypointManager(storage_))
		, route_(new Route(waypointManager_))
		, eventManager_(new EventManager(paramManager_, route_))
		, slowLoop_(NULL)
		, fastLoop_(NULL)
		, zorFcsAdapter_(NULL)
		, wow_(new Wow())
	{
		Manager::Instance().init(storage_, paramManager_, waypointManager_, route_, eventManager_);
		createThreads();
	}

	~App() {
		killThreads();
		delete wow_;
		delete eventManager_;
		delete route_;
		delete waypointManager_;
		delete paramManager_;
		delete storage_;
	}

	void createThreads() {
		slowLoop_ = new Thread("SlowLp", 2048, SCHED_PRIORITY_DEFAULT, new SlowLoop(eventManager_, route_, paramManager_, storage_));
		fastLoop_ = new Thread("FastLp", 2048, SCHED_PRIORITY_DEFAULT, new FastLoop(paramManager_, wow_));
		zorFcsAdapter_ = new Thread("zorfcsAdapter", 2048, SCHED_PRIORITY_DEFAULT, new zorfcsadapter::ZorFcsAdapter());
	}

	void killThreads() {
		delete zorFcsAdapter_;
		delete slowLoop_;
		delete fastLoop_;
	}

	LockableStorage *storage_;
	ParamManager *paramManager_;
	WaypointManager *waypointManager_;
	Route *route_;
	EventManager *eventManager_;
	Thread *slowLoop_;
	Thread *fastLoop_;
	Thread *zorFcsAdapter_;
	InputPin *wow_;
};

App *App::instance_ = NULL;

void publish_armed_status(bool value) {
	struct actuator_armed_s armed;
	armed.armed = value;
	armed.ready_to_arm = true;
	orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed);
	orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
	close(armed_pub);
}

}

extern "C"
int fcs_thread_main(int argc, char *argv[]) {
	argc -= 3;
	argv += 3;

	publish_armed_status(false);

	if (argc > 0) {
		int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);

		if (strcmp(*argv, "cal_gyro") == 0)
			do_gyro_calibration(mavlink_fd);
		else if (strcmp(*argv, "cal_acc") == 0)
			do_accel_calibration(mavlink_fd);
		else if (strcmp(*argv, "cal_mag") == 0)
			do_mag_calibration(mavlink_fd);
		else if (strcmp(*argv, "test") == 0)
			do_test();

		close(mavlink_fd);
		return 0;
	}

	publish_armed_status(true);

	App::create();
	return 0;
}

static void fcs_usage(const char *reason) {
	if (reason)
		fprintf(stderr, "%s\n", reason);

	errx(1, "usage: fcs {start [cal_gyro|cal_acc|cal_mag] | stop | status}\n");
}

/**
 * The fcs deamon app only briefly exists to start the background job.
 * The stack size assigned in the Makefile does only apply to this management task.
 * The actual stack size should be set in the call to task_spawn().
 */
extern "C" 
int fcs_main(int argc, char *argv[]) {
	if (argc < 2)
		fcs_usage("missing command");

	if (!strcmp(argv[1], "start")) {
		if (App::isRunning())
			printf("fcs already running\n");
		else
			task_spawn_cmd("fcs", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 2048, fcs_thread_main, (const char **)argv);
		exit(0);
	}

	if (!strcmp(argv[1], "stop")) {
		if (App::isRunning())
			App::destroy();
		exit(0);
	}

	if (!strcmp(argv[1], "status")) {
		printf("fcs is %s\n", App::isRunning() ? "running" : "not started");
		exit(0);
	}

	fcs_usage("unrecognized command");
	exit(1);
}






